//File Header

#ifndef _MATH_FUNCTIONS_H
#define _MATH_FUNCTIONS_H

#include "Math.h"

//Additional Functions to be used for various calculations
// These calculations include collisions and their reactions

//Half Space Test
float HalfSpaceTest(const Plane plane, vec3f point)
{
	float dot = dot_product(plane.m_vNormal, point)*plane.m_fdot;
	return dot;
}

//Ray to Plane intersection
float RayToPlane(Ray ray, Plane p)
{
	float t;
	t = (p.m_fdot - (dot_product(ray.m_vStart, p.m_vNormal)))/dot_product(p.m_vNormal, ray.m_vDirection);

	return t;
}

int LineSegmentToPlane(vec3f a, vec3f b, Plane p, float &t, vec3f &v)
{
	vec3f ab; ab = b - a;
	if(dot_product(p.m_vNormal, ab) == 0.0)
	{
		//Something is wrong, return a 0 vector
		v = NULL;
		t = -1.0f;
		return -1;
	}

	t = p.m_fdot - dot_product(p.m_vNormal, a);
	t /= dot_product(p.m_vNormal, ab);

	if(t >= 0.0f && t <= 1.0f)
	{
		ab.scale(t);
		v = ab + a;

		return 1;
	}

	//No Intersection
	v = NULL;
	t = -1.0f;

	return 0;
}

bool SegmentToBoxIntersection(vec3f Start, vec3f End, AABB* box, Plane &p, vec3f &CP)
{
	//Create a ray given the start and end points
	Ray r; r.Create(Start, End);

	//Planes
	Plane minX; vec3f minXNorm; minXNorm.make_zero(); minXNorm.x = -1.0f;
	minX.Create(box->m_vMinPos, minXNorm);

	//Halfspace MinX
	if(HalfSpaceTest(minX, Start) >= 0.0f)
	{
		//Check MinX Parallel
		float dot; dot = dot_product(r.m_vDirection, minX.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, minX, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.z >= box->m_vMinPos.z && CP.z <= box->m_vMaxPos.z)
				{
					if(CP.y >= box->m_vMinPos.y && CP.y <= box->m_vMaxPos.y)
					{
						p = minX;				
						return true;
					}
				}
			}
		}
	}

	Plane minZ; vec3f minZNorm; minZNorm.make_zero(); minZNorm.z = -1.0f;
	minZ.Create(box->m_vMinPos, minZNorm);
	//Halfspace MinZ
	if(HalfSpaceTest(minZ, Start) >= 0.0f)
	{
		//Check MinZ Parallel
		float dot; dot = dot_product(r.m_vDirection, minZ.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, minZ, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.x >= box->m_vMinPos.x && CP.x <= box->m_vMaxPos.x)
				{
					if(CP.y >= box->m_vMinPos.y && CP.y <= box->m_vMaxPos.y)
					{
						p = minZ;
						return true;
					}
				}
			}
		}
	}

	Plane maxZ; vec3f maxZNorm; maxZNorm.make_zero(); maxZNorm.z = 1.0f;
	maxZ.Create(box->m_vMaxPos, maxZNorm);

	//Halfspace MaxZ
	if(HalfSpaceTest(maxZ, Start) >= 0.0f)
	{
		//Check MaxX Parallel
		float dot; dot = dot_product(r.m_vDirection, maxZ.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, maxZ, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.x >= box->m_vMinPos.x && CP.x <= box->m_vMaxPos.x)
				{
					if(CP.y >= box->m_vMinPos.y && CP.y <= box->m_vMaxPos.y)
					{
						p = maxZ;
						return true;
					}
				}
			}
		}
	}

	Plane maxX; vec3f maxXNorm; maxXNorm.make_zero(); maxXNorm.x = 1.0f;
	maxX.Create(box->m_vMaxPos, maxXNorm);
	
	//Halfspace MaxX
	if(HalfSpaceTest(maxX, Start) >= 0.0f)
	{
		//Check MinX Parallel
		float dot; dot = dot_product(r.m_vDirection, maxX.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, maxX, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.z >= box->m_vMinPos.z && CP.z <= box->m_vMaxPos.z)
				{
					if(CP.y >= box->m_vMinPos.y && CP.y <= box->m_vMaxPos.y)
					{
						p = maxX;
						return true;
					}
				}
			}
		}
	}	

	//MinY
	Plane minY; vec3f minYNorm; minYNorm.make_zero(); minYNorm.y = -1.0f;
	minY.Create(box->m_vMinPos, minYNorm);
	
	//Halfspace MaxX
	if(HalfSpaceTest(minY, Start) >= 0.0f)
	{
		//Check MinX Parallel
		float dot; dot = dot_product(r.m_vDirection, minY.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, minY, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.z >= box->m_vMinPos.z && CP.z <= box->m_vMaxPos.z)
				{
					if(CP.x >= box->m_vMinPos.x && CP.x <= box->m_vMaxPos.x)
					{
						p = minY;
						return true;
					}
				}
			}
		}
	}	

	//MinY
	Plane maxY; vec3f maxYNorm; maxYNorm.make_zero(); maxYNorm.y = 1.0f;
	maxY.Create(box->m_vMaxPos, maxYNorm);
	
	//Halfspace MaxX
	if(HalfSpaceTest(maxY, Start) >= 0.0f)
	{
		//Check MinX Parallel
		float dot; dot = dot_product(r.m_vDirection, maxY.m_vNormal);

		if(dot < 0)
		{
			float f;
			if(LineSegmentToPlane(Start, End, maxY, f, CP) == 1)
			{
				//Now make sure that line goes through the box
				if(CP.z >= box->m_vMinPos.z && CP.z <= box->m_vMaxPos.z)
				{
					if(CP.x >= box->m_vMinPos.x && CP.x <= box->m_vMaxPos.x)
					{
						p = maxY;
 						return true;
					}
				}
			}
		}
	}	


	//Outside bounds and didn't collide with anything
	return false;

}

float Get3dDistance( D3DXVECTOR3 c1, D3DXVECTOR3 c2 )
{
	//(Dx*Dx+Dy*Dy+Dz*Dz)^.5 
	float dx = c2.x - c1.x;
	float dy = c2.y - c1.y;
	float dz = c2.z - c1.z;

	return sqrt((float)(dx * dx + dy * dy + dz * dz));
}

//Helper function for creating AABBs with Min & Max 
// using Direct X format
AABB CreateAABB(D3DXVECTOR3 pos, D3DXVECTOR3 dim)
{
	AABB temp;
	vec3f min, max;

	min.x = pos.x - dim.x; min.y = pos.y - dim.y; min.z = pos.z - dim.z;
	max.x = pos.x + dim.x; max.y = pos.y + max.y; max.z = pos.z + dim.z;

	temp.Create(min, max, -1, -1);
	return temp;
}

#endif